/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_RPY_HPP
#define RW_MATH_RPY_HPP

/**
 * @file RPY.hpp
 */
#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Rotation3DVector.hpp>
#include <rw/math/Vector3D.hpp>
#endif
namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A class for representing Roll-Pitch-Yaw Euler angle rotations.
     */
    template< class T = double > class RPY : public Rotation3DVector< T >
    {
      public:
        /**
         * @brief Constructs rotation in which all elements are initialized to 0
         */
        RPY () : _rpy (0.0, 0.0, 0.0) {}

        /**
         * @brief Constructs an initialized roll-pitch-yaw euler angle vector
         * @param roll Rotation around z
         * @param pitch Rotation around y
         * @param yaw Rotation around x
         */
        RPY (T roll, T pitch, T yaw) : _rpy (roll, pitch, yaw) {}

        /**
         * @brief Constructs an RPY object initialized according to the specified Rotation3D
         *
         * \f$ \beta = arctan2(-r_{31},\sqrt{r_{11}^2+r_{21}^2}) \f$
         * \f$ \alpha = arctan2(r_{21}/cos(\beta), r_{11}/cos(\beta)) \f$
         * \f$ \beta = arctan2(r_{32}/cos(\beta), r_{33}/cos(\beta))) \f$
         *
         * @param R [in] A 3x3 rotation matrix \f$ \mathbf{R} \f$
         *
         * @param epsilon [in] Value specifying the value for which \f$
         * cos(\beta)\f$ is assumed 0 and the special case solution assuming
         * \f$\alpha=0, \beta=\pi/2 and \gamma = arctan2(r_{21}, r_{22})\f$ is
         * to be used.
         */
        explicit RPY (const Rotation3D< T >& R, T epsilon = 1e-5);

        /**
         * @copydoc Rotation3DVector::toRotation3D
         */
        const Rotation3D< T > toRotation3D () const;
#if !defined(SWIG)
        /**
         * @brief Returns reference to the element
         *
         * @param index [in] index of element
         *
         * @return reference to the element
         */
        T& operator() (size_t index) { return _rpy (index); }

        /**
         * @brief Returns a const reference the an element
         *
         * @param index [in] index of element
         *
         * @return const reference to the element
         */
        const T& operator() (size_t index) const { return _rpy (index); }

        /**
         * @brief Returns a const reference the an element
         *
         * @param i [in] index of element
         *
         * @return const reference to the element
         */
        const T& operator[] (size_t i) const { return (*this) (i); }

        /**
         * @brief Returns reference to the element
         *
         * @param i [in] index of element
         *
         * @return reference to the element
         */
        T& operator[] (size_t i) { return (*this) (i); }

#else
        ARRAYOPERATOR (T);
#endif

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] RPY to compare with
         * @return True if equal.
         */
        bool operator== (const RPY< T >& rhs) const
        {
            return (_rpy (0) == rhs (0) && _rpy (1) == rhs (1) && _rpy (2) == rhs (2));
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] RPY to compare with
         * @return True if not equal.
         */
        bool operator!= (const RPY< T >& rhs) const { return !(*this == rhs); }

        /**
         * @brief size of this RPY.
         * @return the value 3
         */
        size_t size () const { return 3; }

#if !defined(SWIG)
        /**
         * @brief Ouputs RPY to stream
         * @param os [in/out] stream to use
         * @param rpy [in] rpy rotation
         * @return the resulting stream
         */
        friend std::ostream& operator<< (std::ostream& os, const RPY< T >& rpy)
        {
            return os << "RPY {" << rpy (0) << ", " << rpy (1) << ", " << rpy (2) << "}";
            // return os << rpy._rpy;
        }

#else
    TOSTRING (rw::math::RPY<T>);
#endif 
      private:
        Vector3D< T > _rpy;
    };

    /**
     * @brief Casts RPY<T> to RPY<Q>
     *
     * @param rpy [in] RPY with type T
     *
     * @return RPY with type Q
     */
    template< class Q, class T > const RPY< Q > cast (const RPY< T >& rpy)
    {
        return RPY< Q > (
            static_cast< Q > (rpy (0)), static_cast< Q > (rpy (1)), static_cast< Q > (rpy (2)));
    }
#if !defined(SWIG)
    extern template class rw::math::RPY< double >;
    extern template class rw::math::RPY< float >;
#else

#if SWIG_VERSION < 0x040000
    SWIG_DECLARE_TEMPLATE (RPYd, rw::math::RPY< double >);
    ADD_DEFINITION (RPYd, RPY,sdurw_math)
#else
    SWIG_DECLARE_TEMPLATE (RPY, rw::math::RPY< double >);
#endif
    SWIG_DECLARE_TEMPLATE (RPYf, rw::math::RPY< float >);

#endif
    using RPYd = RPY< double >;
    using RPYf = RPY< float >;

    /*@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::RPY
         */
        template<>
        void write (const rw::math::RPY< double >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::RPY
         */
        template<>
        void write (const rw::math::RPY< float >& sobject, rw::common::OutputArchive& oarchive,
                    const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::RPY
         */
        template<>
        void read (rw::math::RPY< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::RPY
         */
        template<>
        void read (rw::math::RPY< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard
